clc
clear all
%close all

syms x1 x2 x3 x4 x5 x6;%位姿
syms l1 l2 l3 l4 l5 l6;%杆长

syms b1_x b1_y b1_z;
syms b2_x b2_y b2_z;
syms b3_x b3_y b3_z;
syms b4_x b4_y b4_z;
syms b5_x b5_y b5_z;
syms b6_x b6_y b6_z;

syms P1_x P1_y P1_z;
syms P2_x P2_y P2_z;
syms P3_x P3_y P3_z;
syms P4_x P4_y P4_z;
syms P5_x P5_y P5_z;
syms P6_x P6_y P6_z;

%零初始位置时静平台位置
%零初始位置时静平台位置
B1=[b1_x; b1_y; b1_z];
B2=[b2_x; b2_y; b2_z];
B3=[b3_x; b3_y; b3_z];
B4=[b4_x; b4_y; b4_z];
B5=[b5_x; b5_y; b5_z];
B6=[b6_x; b6_y; b6_z];
%零初始位置时动平台位置
P1=[P1_x; P1_y; P1_z];
P2=[P2_x; P2_y; P2_z];
P3=[P3_x; P3_y; P3_z];
P4=[P4_x; P4_y; P4_z];
P5=[P5_x; P5_y; P5_z];
P6=[P6_x; P6_y; P6_z];
% x1=x(1);
% x2=x(2);
% x3=x(3);
a=x4;
b=x5;
g=x6;
% l1=Length(1);
% l2=Length(2);
% l3=Length(3);
% l4=Length(4);
% l5=Length(5);
% l6=Length(6);
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
f1=(norm(X+R*P1-B1))^2-l1^2;
f2=(norm(X+R*P2-B2))^2-l2^2;
f3=(norm(X+R*P3-B3))^2-l3^2;
f4=(norm(X+R*P4-B4))^2-l4^2;
f5=(norm(X+R*P5-B5))^2-l5^2;
f6=(norm(X+R*P6-B6))^2-l6^2;

df_f1_dx1 = diff(f1, x1); % df1/dx1
df_f1_dx2 = diff(f1, x2); % df1/dx2
df_f1_dx3 = diff(f1, x3); % df1/dx3
df_f1_dx4 = diff(f1, x4); % df1/dx4
df_f1_dx5 = diff(f1, x5); % df1/dx5
df_f1_dx6 = diff(f1, x6); % df1/dx6

df_f2_dx1 = diff(f2, x1); % df2/dx1
df_f2_dx2 = diff(f2, x2); % df2/dx2
df_f2_dx3 = diff(f2, x3); % df1/dx3
df_f2_dx4 = diff(f2, x4); % df1/dx4
df_f2_dx5 = diff(f2, x5); % df1/dx5
df_f2_dx6 = diff(f2, x6); % df1/dx6

df_f3_dx1 = diff(f3, x1); % df3/dx1
df_f3_dx2 = diff(f3, x2); % df2/dx2
df_f3_dx3 = diff(f3, x3); % df1/dx3
df_f3_dx4 = diff(f3, x4); % df1/dx4
df_f3_dx5 = diff(f3, x5); % df1/dx5
df_f3_dx6 = diff(f3, x6); % df1/dx6

df_f4_dx1 = diff(f4, x1); % df4/dx1
df_f4_dx2 = diff(f4, x2); % df2/dx2
df_f4_dx3 = diff(f4, x3); % df1/dx3
df_f4_dx4 = diff(f4, x4); % df1/dx4
df_f4_dx5 = diff(f4, x5); % df1/dx5
df_f4_dx6 = diff(f4, x6); % df1/dx6

df_f5_dx1 = diff(f5, x1); % df5/dx1
df_f5_dx2 = diff(f5, x2); % df2/dx2
df_f5_dx3 = diff(f5, x3); % df1/dx3
df_f5_dx4 = diff(f5, x4); % df1/dx4
df_f5_dx5 = diff(f5, x5); % df1/dx5
df_f5_dx6 = diff(f5, x6); % df1/dx6

df_f6_dx1 = diff(f6, x1); % df6/dx1
df_f6_dx2 = diff(f6, x2); % df2/dx2
df_f6_dx3 = diff(f6, x3); % df1/dx3
df_f6_dx4 = diff(f6, x4); % df1/dx4
df_f6_dx5 = diff(f6, x5); % df1/dx5
df_f6_dx6 = diff(f6, x6); % df1/dx6

J= [df_f1_dx1,df_f1_dx2,df_f1_dx3,df_f1_dx4,df_f1_dx5,df_f1_dx6;
    df_f2_dx1,df_f2_dx2,df_f2_dx3,df_f2_dx4,df_f2_dx5,df_f2_dx6;
    df_f3_dx1,df_f3_dx2,df_f3_dx3,df_f3_dx4,df_f3_dx5,df_f3_dx6;
    df_f4_dx1,df_f4_dx2,df_f4_dx3,df_f4_dx4,df_f4_dx5,df_f4_dx6;
    df_f5_dx1,df_f5_dx2,df_f5_dx3,df_f5_dx4,df_f5_dx5,df_f5_dx6;
    df_f6_dx1,df_f6_dx2,df_f6_dx3,df_f6_dx4,df_f6_dx5,df_f6_dx6;   
];
simplify(J)

